#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Sensor, S4,     Ultrasonic,     sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     LeftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     RightMotor,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int white = 1000;
int black = -1000;

#define CHECK_WHITE_LINE if(SensorValue[Light]>=WHITE)\
								          {\
								          	findWhiteLine = true;\
								          	break;\
								          }\

void turnL (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void Forward (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= power;
  wait1Msec(duration);
}

void turnR (int power, int duration)
{
  motor[LeftMotor]= power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

void Backward (int power, int duration)
{
  motor[LeftMotor]= -1*power;
  motor[RightMotor]= -1*power;
  wait1Msec(duration);
}

void ProportionalLineFollowing ()
{
	while(SensorValue[Ultrasonic] <= 50)
	{
		while(SensorValue[Light] >= 0 && SensorValue[Light] <= 20)
		{
			motor[LeftMotor] = 0;
			motor[RightMotor] = 50;
			wait1Msec(50);
		}
		while(SensorValue[Light] > 20 && SensorValue[Light] <= 40)
		{
			motor[LeftMotor] = 12;
			motor[RightMotor] = 38;
			wait1Msec(50);
		}
		while(SensorValue[Light] > 40 && SensorValue[Light] <= 60)
		{
			motor[LeftMotor] = 25;
			motor[RightMotor] = 25;
			wait1Msec(50);
		}
		while(SensorValue[Light] > 60 && SensorValue[Light] <= 80)
		{
			motor[LeftMotor] = 38;
			motor[RightMotor] = 12;
			wait1Msec(50);
		}
		while(SensorValue[Light] > 80 && SensorValue[Light] <= 100)
		{
			motor[LeftMotor] = 50;
			motor[RightMotor] = 0;
			wait1Msec(50);
		}
	}
}

int BeaconPost = 0;

void FindBeaconPost()
{
	if (SensorValue[IR1200] == 5)
	{
		BeaconPost = 2;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else if (SensorValue[IR1200] > 5)
	{
		BeaconPost = 3;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else if (SensorValue[IR1200] < 5)
	{
		BeaconPost = 1;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else
	{
		while(BeaconPost == 0)
		{
			BeaconPost = 0;
			nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
		}
	}
}

void GoToBeacon()
{
	if (BeaconPost == 1)
	{
		turnL(50,300);
		Forward(100,1000);
		turnR(50,600);
		while(SensorValue[Ultrasonic] >= 30)
		{
			Forward(30,50);
		}
		// Here the servo programming will be inserted.
	}
	else if(BeaconPost == 2)
	{
		while(SensorValue[Light] < black + 10 )
		{
			Forward(5,50);
		}
		while(SensorValue[Light] > white - 10)
		{
			turnL(30,1000);
			//Here the servo programming will be inserted.
		}
	}
	else if(BeaconPost == 3)
	{
		turnR(50,300);
		Forward(100,1000);
		turnL(30,600);
		while(SensorValue[Ultrasonic] >= 55)
		{
			Forward(50,50);
		}
		// Here the servo programming will be inserted.
	}
	else
	{
		while (BeaconPost == 0)
		{
			FindBeaconPost();
		}
	}
}

void LightSensorCalibration()
{
	nxtDisplayCenteredTextLine(3, "BLACK");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3,"RIGHT");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3,"FIRST");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3, "WHITE");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3,"LEFT");
	wait1Msec(1000);
	nxtDisplayCenteredTextLine(3,"SECOND");
	wait1Msec(1000);
	while(nNxtButtonPressed != 3)
  {
	  if(nNxtButtonPressed == 1)
	  {
	  	black = SensorValue[Light];
			nxtDisplayCenteredTextLine(3,"BLACK %d", black);
	  }
	  else if(nNxtButtonPressed == 2)
	  {
	  	white = SensorValue[Light];
	  	nxtDisplayCenteredTextLine(3, "WHITE %d", white);
	  }
	}
}

task main()
{
//	LightSensorCalibration();
	bool findBeacon = false;
	bool findWhiteLine = false;
	bFloatDuringInactiveMotorPWM = false;
	while(SensorValue[Ultrasonic] > 65)
	{
		nxtDisplayCenteredBigTextLine(3,"%d", SensorValue[Ultrasonic]);
		Forward(100,50);
	}
	/*while(!findBeacon)
	{
	   switch (SensorValue[IR1200])
	   {
	     case 1:
	     Backward(15,50);
	     break;

	     case 2:
	     turnL(30,900);
	     findBeacon = true;
	     break;

	     case 0:
	     case 3:
	     case 4:
	     case 5:
	     case 6:
	     case 7:
	     Forward(50,50);
	     break;

	     default:
	   	 break;
	   }
  }*/

  //END IRFOLLOW
	FindBeaconPost();
	Backward(100,2000);
	GoToBeacon();
/*	while(!findWhiteLine && SensorValue[Touch]==0)
	{
		while(SensorValue[IR1200] == 1)
		{
			turnL(15,2150);
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200] == 2)
		{
			turnL(15,2000);
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200] == 3)
		{
			turnL(15,1250);
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200] == 4)
		{
			turnL(15,500);
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200] == 5)
		{
			Forward(15,50)
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200] == 6)
		{
			turnR(15,500);
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200] == 7)
		{
			turnR(15,1250);
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200] == 8)
		{
			turnR(15,2000);
			CHECK_WHITE_LINE;
		}
		while(SensorValue[IR1200] == 9)
		{
			turnR(15,2100);
			CHECK_WHITE_LINE;
		}
	}
	*/
}
